# -*- coding: utf-8 -*-
import rospy
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
from v2x_msgs.msg import RSM, ParticipantData

# Connect to client
client = carla.Client('192.168.3.157', 2000)
client.set_timeout(2.0)

world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps = "WGS84")     
def xy2gps(x,y,elevation):
    dis = math.sqrt(math.pow(x,2)+math.pow(y,2))
    azm = math.degrees(math.atan2(-y,x))
    #设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768,43.8334643,azm,dis)
    return lon*10000000, lat*10000000, elevation

def get_gps(actor):
    t = actor.get_transform()
    return xy2gps(t.location.x,t.location.y,t.location.z)

def get_speed(actor):
    v = actor.get_velocity()
    speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))  # km/h
    return speed

def talker():

    pub = rospy.Publisher('rsm', RSM, queue_size=10)
    rospy.init_node('rsm', anonymous=True)
    rate = rospy.Rate(1) # 1hz
    while not rospy.is_shutdown():
        rsm = RSM()
        rsm.msgcnt = 1
        rsm.id = "1"
        rsm.rsu_lon, rsm.rsu_lat, eve= xy2gps(95,205,0) 
        ptlist = []
        for actor in actors:
            if 'vehicle' in actor.type_id:
                pData = ParticipantData()
                pData.ParticipantType = 1
                pData.ptcId = actor.id
                pData.SourceType = 1
                pData.lon, pData.lat, ignored = get_gps(actor)

                # velocity
                v = actor.get_velocity()
                speed=3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
                pData.speed = speed
                # transform
                t = actor.get_transform()
                yaw = t.rotation.yaw 
                if(yaw < -90):
                    yaw = 450 + yaw
                else:
                    yaw = 90 + yaw                
                pData.heading = yaw

                pData.length = 2
                pData.width = 1
                pData.height = 1

                ptlist.append(pData)
        rsm.ptlist = ptlist

        pub.publish(rsm)

        rospy.loginfo("published!")
        rate.sleep()

talker()